#include <eigen_conversions/eigen_msg.h>
#include <ros/ros.h>
#include <pcl/common/transforms.h>
#include <pcl/common/common.h>
//#include <pcl/io/ply_io.h>
#include <pcl/io/pcd_io.h>

#include <pcl_conversions/pcl_conversions.h>
#include <pcl_ros/point_cloud.h>
#include <tf2_ros/transform_listener.h>
#include "ground_segmentation/ground_segmentation.h"
#include "pcl/filters/filter.h"
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/filters/crop_box.h>
#include "glog/logging.h"
#include "tools/file_manager.hpp"
#include <visualization_msgs/MarkerArray.h>
#include <Eigen/Dense>
#include <nav_msgs/GridCells.h>
#include <set>

//VoxelGrid include
#include <nav_msgs/Odometry.h>
#include <tf/LinearMath/Quaternion.h>
#include <tf/LinearMath/Matrix3x3.h>
#include <std_msgs/UInt8MultiArray.h>
#include <cv_bridge/cv_bridge.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include "msgs/BboxList.h"
#include "msgs/Bbox.h"
#include "msgs/PCArray.h"

#include "cam_object_class.h"

//#include "msg"
using namespace Eigen;

using namespace std;
string WORK_SPACE_PATH;
string CameraBoxTopic;
string LidarBoxTopic;

ros::Publisher pub_UInt8MultiArray_;
ros::Publisher pub_MergeLidarCameraBox_;
ros::Subscriber sub_LidarBox_;
ros::Subscriber sub_Lidar_;
ros::Subscriber sub_CameraBox_;
ros::Subscriber sub_PCArray_obj_;
ros::Publisher pub_vertical_view_ ;
ros::Publisher pub_vertical_view_text_;
ros::Publisher pub_vertical_view_bbox_;
ros::Publisher pub_vertical_view_coordinate_;


Eigen::Matrix3d lidar2camera_R;
Eigen::Vector3d lidar2camera_T;

//deepcamera ->camera
Eigen::Matrix3d dcamera2lidar_R;
Eigen::Vector3d dcamera2lidar_T;

double dcx = 425.70074462890625;
double dcy = 234.9811248779297;
double dfx = 429.1876220703125;
double dfy = 429.1876220703125;
float kScaleFactor = 1000;
float DeepCameraHigh = 0.25;
bool newCameraBox = false;
bool newLidarBox = false;
float fx = 1264.252,fy = 1264.52,cx = 650.92,cy = 536.64;

int camera_cols = 1280,camera_rows = 1024;
int num_text = 0;

//visualization_msgs::MarkerArray::Ptr
// todo add time alignment
vector<vector<pair<pcl::PointXY,pcl::PointXY>>> LidarBoxArraySeq;
vector<pair<pcl::PointXY,pcl::PointXY>> LidarBoxArray;
vector<msgs::BboxList> CamBoxArraySeq;
//vector<std_msgs::Header> LidarBoxStamp;
//msgs::BboxList CamBoxList;
//std_msgs::Header CamStamp;
std_msgs::Header PC_Stamp;
visualization_msgs::MarkerArray marker_array_merge;

pcl::PointCloud<pcl::PointXYZ>::Ptr laserCloud_temp(new pcl::PointCloud<pcl::PointXYZ>());
std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> clusters_ ;
std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> cloudClusters;

std_msgs::UInt8MultiArray out;
msgs::PCArray PC_Array_obj_;
vector<pcl::PointCloud<pcl::PointXYZ>> PC_Array_obj_vector;  //存储聚类得到的点云


template<typename T>
void float2u8Arry(T &out, float floatdata)
{
    uint8_t uarray[4];
    *(float *)uarray = *&floatdata;  //  使用 float 数据的格式解析 uarray 指针地址（相当于强制类型转换），然后对该地址进行赋值操作

    for(int i = 0; i<=3; ++i){
        out.data.push_back(uarray[i]);
    }

}

template<typename T>
void int162u8Arry(T &out, int16_t floatdata)
{
    uint8_t uarray[2];
    *(int16_t *)uarray = *&floatdata;  //  使用 float 数据的格式解析 uarray 指针地址（相当于强制类型转换），然后对该地址进行赋值操作

    for(int i = 0; i<=1; ++i){
        out.data.push_back(uarray[i]);
    }
}

template<typename T>
void uint162u8Arry(T &out, uint16_t floatdata)
{
    uint8_t uarray[2];
    *(uint16_t *)uarray = *&floatdata;  //  使用 float 数据的格式解析 uarray 指针地址（相当于强制类型转换），然后对该地址进行赋值操作

    for(int i = 0; i<=1; ++i){
        out.data.push_back(uarray[i]);
    }
}

template<typename T>
void TotalLen2u8Arry(T &out, uint16_t floatdata)
{
    uint8_t uarray[2];
    *(uint16_t *)uarray = *&floatdata;  //  使用 float 数据的格式解析 uarray 指针地址（相当于强制类型转换），然后对该地址进行赋值操作

    for(int i = 0; i<=1; ++i){
        out.data[i] = uarray[i];
    }
}

void PublishROSMsg(visualization_msgs::MarkerArray &marker_array){
    std_msgs::UInt8MultiArray out;
    pcl::PointCloud<pcl::PointXYZ> thisCloud;
    pcl::PointXYZ thisPoint;

    uint8_t obstacle_num = 0;
    uint162u8Arry(out, 0);//total_length
    out.data.push_back(0);//total_object_num

    int MarkSize = marker_array.markers.size();

    //extract 8 conner points of obstacle
    for(visualization_msgs::Marker &mark : marker_array.markers){
        float scalex = mark.scale.x/2;
        float scaley = mark.scale.y/2;
        float scalez = mark.scale.z/2;

        for(int signx = -1;signx <=1;signx += 2){
            for(int signy = -1;signy<=1;signy +=2){
                for(int signz = -1;signz<=1;signz += 2){
                    thisPoint.x = mark.pose.position.x + signx*scalex;
                    thisPoint.y = mark.pose.position.y + signy*scaley;
                    thisPoint.z = mark.pose.position.z + signz*scalez;
                    thisCloud.push_back(thisPoint);
                }
            }
        }
    }

    //convert from map->imu->lidar->camera->pixel
    int PointSize = thisCloud.size();
    for(int i =0;i<PointSize;i++){
        /*thisPoint = thisCloud.points[i];
        //map to imu
        float PX4 = thisPoint.x - vehicleX;
        float PY4 = thisPoint.y - vehicleY;
        float PZ4 = thisPoint.z - vehicleZ;

        float PX3 = PX4;
        float PY3 = cosVehicleRoll*PY4 + sinVehicleRoll*PZ4;
        float PZ3 = cosVehicleRoll*PZ4 + sinVehicleRoll*PY4;

        float PX2 = PX3*cosVehiclePitch - PZ3*sinVehiclePitch;
        float PY2 = PY3;
        float PZ2 = PZ3*sinVehiclePitch - PZ3*cosVehiclePitch;

        float PX1 = PX2*cosVehicleYaw + PY2*sinVehicleYaw;
        float PY1 = - PX2*sinVehicleYaw + PY2*cosVehicleYaw ;
        float PZ1 = PZ2;

        //imu->lidar
        PX1 = PX1 - 0.1;
        PZ1 = PZ1 - 0.15;
        float temp;
        temp = PY1;
        PY1 = PX1;
        PX1 = -temp;*/

        float PX1 = thisCloud.points[i].x;
        float PY1 = thisCloud.points[i].y;
        float PZ1 = thisCloud.points[i].z;

        //lidar->camera
//        Eigen::Matrix3d lidar2camera_R;
//        lidar2camera_R<<0.9994,-0.006152,0.034226,-0.034151,0.011869,0.99935,-0.0065542,-0.99991,0.01165;
        Eigen::Vector3d v3d;
        v3d<<PX1,PY1,PZ1;
//        lidar2camera_T<<-0.00305,0.42377,-0.09468;
        v3d = lidar2camera_R.transpose()*v3d + (-lidar2camera_R.transpose()*lidar2camera_T);

        //normalize
        v3d.x() = v3d.x()/v3d.z();
        v3d.y() = v3d.y()/v3d.z();
        v3d.z() = 1;

        //distort

        //camera->pixel
        thisCloud.points[i].x = fx*v3d.x()+cx;
        thisCloud.points[i].y = fy*v3d.y()+cy;
        thisCloud.points[i].z = 1;
    }

    int pointsize = thisCloud.size();

    for(int i = 0 ;i<pointsize;i += 8){
        uint162u8Arry(out, 21);//Length class:1,conficence:4,outline:4*2*2
        int datastart = out.data.size()-2;
        out.data.push_back(0);//class
        float2u8Arry(out, 0);//confidence

        int16_t minx,miny,maxx,maxy;
        minx = maxx = thisCloud.points[i].x;
        miny = maxy = thisCloud.points[i].y;
        for(int j = i+1;j<i+4;j++){
            if(thisCloud.points[j].x>maxx)maxx=thisCloud.points[j].x;
            if(thisCloud.points[j].x<minx)minx=thisCloud.points[j].x;
            if(thisCloud.points[j].y>maxy)maxy=thisCloud.points[j].y;
            if(thisCloud.points[j].y<miny)miny=thisCloud.points[j].y;
        }

        int162u8Arry(out,minx);//outline
        int162u8Arry(out,miny);
        int162u8Arry(out,maxx);
        int162u8Arry(out,maxy);

        minx = maxx = thisCloud.points[i+4].x;
        miny = maxy = thisCloud.points[i+4].y;
        for(int j = i+5;j<i+8;j++){
            if(thisCloud.points[j].x>maxx)maxx=thisCloud.points[j].x;
            if(thisCloud.points[j].x<minx)minx=thisCloud.points[j].x;
            if(thisCloud.points[j].y>maxy)maxy=thisCloud.points[j].y;
            if(thisCloud.points[j].y<miny)miny=thisCloud.points[j].y;
        }

        int162u8Arry(out,minx);//outline
        int162u8Arry(out,miny);
        int162u8Arry(out,maxx);
        int162u8Arry(out,maxy);
        obstacle_num++;
        out.data[datastart] = out.data.size() - (datastart+2);//set length of obstacle
    }

    TotalLen2u8Arry(out,out.data.size()-2);
    out.data[2] = obstacle_num;
    pub_UInt8MultiArray_.publish(out);

}



std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> clusterPoints(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_)
{
    //std::cout << " clusterPoints is running...... " << std::endl;
    clusters_.clear();

    pcl::search::KdTree<pcl::PointXYZ>::Ptr cluster_tree (new pcl::search::KdTree<pcl::PointXYZ>);
    cluster_tree->setInputCloud (cloud_);

    std::vector<pcl::PointIndices> cluster_indices;
    pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
    ec.setClusterTolerance (0.4); // 40cm
    ec.setMinClusterSize (10);
    ec.setMaxClusterSize (1000);
    ec.setSearchMethod (cluster_tree);
    ec.setInputCloud (cloud_);
    ec.extract (cluster_indices);

    //int j = 0;
    for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
    {
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZ>);
        for (std::vector<int>::const_iterator pit = it->indices.begin(); pit != it->indices.end(); pit++)
        {
            cloud_cluster->points.push_back (cloud_->points[*pit]); //*
        }
        clusters_.push_back(cloud_cluster);

        cloud_cluster->width = cloud_cluster->points.size();
        cloud_cluster->height = 1;
        cloud_cluster->is_dense = true;

    }
    return clusters_;
}


std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> EuclideanClustering(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_Eu) {
    //std::cout << " EuclideanClustering is running...... " << std::endl;
    // Time clustering process

    ros::Time startTime = ros::Time::now();

    std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> clusters = clusterPoints(cloud_Eu);

    ros::Time endTime = ros::Time::now();

    double elapsedTime = (endTime - startTime).toSec();
    std::cout << "EuclideanClustering took " << elapsedTime << " Seconds and found " << clusters.size() << " clusters" << std::endl;

    return clusters;
}




void LidarBoxHandler(const visualization_msgs::MarkerArrayConstPtr &BoxMarkArray){
    ROS_INFO("LidarBoxHandler");
//    if(LidarBoxStamp.size() == 20)//only record 20 nearest Stamp
//        LidarBoxStamp.erase(LidarBoxStamp.begin());
//    if(LidarBoxArraySeq.size() == 20)
//        LidarBoxArraySeq.erase(LidarBoxArraySeq.begin());
    marker_array_merge = *BoxMarkArray;
    //LidarBoxStamp.push_back(BoxMarkArray->markers[0].header);
    PC_Stamp.stamp = BoxMarkArray->markers[0].header.stamp;

    pcl::PointXYZ thisPoint;
    pcl::PointCloud<pcl::PointXYZ> thisCloud;
    pcl::PointXY l2c_start,l2c_end;

    int BoxSize = BoxMarkArray->markers.size();
    ROS_INFO("BoxSize = %d",BoxSize);
    //extract 8 corner of ObjectBox
    for(int i = 0;i<BoxSize;i++){
        float scalex = BoxMarkArray->markers[i].scale.x/2;
        float scaley = BoxMarkArray->markers[i].scale.y/2;
        float scalez = BoxMarkArray->markers[i].scale.z/2;

//        for(int signy = -1;signy <=1;signy += 2){
            //only use front 4 corner  //y轴超前
            int signy = -1;
            for(int signx = -1;signx<=1;signx +=2){
                for(int signz = -1;signz<=1;signz += 2){
                    thisPoint.x = BoxMarkArray->markers[i].pose.position.x + signx*scalex;
                    thisPoint.y = BoxMarkArray->markers[i].pose.position.y + signy*scaley;
                    thisPoint.z = BoxMarkArray->markers[i].pose.position.z + signz*scalez;
                    thisCloud.push_back(thisPoint);
                }
            }
//        }
    }

    //lidar->camera->pixel
    int PointSize = thisCloud.size();
    ROS_INFO("PointSize = %d",PointSize);
    for(int i =0;i<PointSize;i++){
        //thisPoint = thisCloud.points[i];

        float PX1 = thisCloud.points[i].x;
        float PY1 = thisCloud.points[i].y;
        float PZ1 = thisCloud.points[i].z;

        //lidar->camera
        Eigen::Vector3d v3d;
        v3d<<PX1,PY1,PZ1;
        v3d = lidar2camera_R.transpose()*v3d + (-lidar2camera_R.transpose()*lidar2camera_T);

        //normalize
        v3d.x() = v3d.x()/v3d.z();
        v3d.y() = v3d.y()/v3d.z();
        v3d.z() = 1;

        //distort

        //camera->pixel
        thisCloud.points[i].x = fx*v3d.x()+cx;
        thisCloud.points[i].y = fy*v3d.y()+cy;
        thisCloud.points[i].z = 1;
    }

    int pointsize = thisCloud.points.size();
    ROS_INFO("pointsize = %d",pointsize);
    for(int i = 0 ;i<pointsize;i += 4){    //为什么是i += 8？ 而不是i += 4？

        int16_t minx,miny,maxx,maxy;
        minx = maxx = thisCloud.points[i].x;
        miny = maxy = thisCloud.points[i].y;
        for(int j = i+1;j<i+4;j++){
            if(thisCloud.points[j].x>maxx)maxx=thisCloud.points[j].x;
            if(thisCloud.points[j].x<minx)minx=thisCloud.points[j].x;
            if(thisCloud.points[j].y>maxy)maxy=thisCloud.points[j].y;
            if(thisCloud.points[j].y<miny)miny=thisCloud.points[j].y;
        }
        if(minx<0 || miny<0 ||maxx >camera_cols || maxy>camera_rows)
            continue;

        l2c_start.x = minx;l2c_start.y = miny;
        l2c_end.x = maxx;l2c_end.y = maxy;

        LidarBoxArray.push_back(pair(l2c_start,l2c_end));
    }
    //LidarBoxArraySeq.push_back(LidarBoxArray);
    newLidarBox = true;
}

void LidarHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloud2) {


    pcl::fromROSMsg(*laserCloud2, *laserCloud_temp);
}

void CameraBoxHandler(const msgs::BboxList &CamBoxListMsg){

    while (CamBoxArraySeq.size() >= 30)//only record 20 nearest Stamp
        CamBoxArraySeq.erase(CamBoxArraySeq.begin());

    msgs::Bbox CamBox;
    msgs::BboxList CamBoxList_;
    for(msgs::Bbox bbox: CamBoxListMsg.Bboxlist){
        CamBox.conf = bbox.conf;
        CamBox.label = bbox.label;
        CamBox.bbox = bbox.bbox;
        CamBoxList_.Bboxlist.push_back(CamBox);
    }
    CamBoxList_.header.stamp = CamBoxListMsg.header.stamp;
    CamBoxArraySeq.push_back(CamBoxList_);

    // CamStamp = CamBoxListMsg.header;

    newCameraBox = true;
//    CamBoxArray = CamBox.Bboxlist;
//    CamStamp = ;
}
//void CameraBoxHandler(const Objec)

void PCArrayHandler(const msgs::PCArray &PCArrayMsg){
    //PC_Array_obj_ = PCArrayMsg;
    int PCArray_num = PCArrayMsg.PC_Array.size();
    for (int i = 0; i < PCArray_num; ++i) {
        pcl::PointCloud<pcl::PointXYZ> lasercloud_pc;
        pcl::fromROSMsg(PCArrayMsg.PC_Array[i], lasercloud_pc);
        PC_Array_obj_vector.push_back(lasercloud_pc);
    }
}


int main(int argc, char **argv) {
    ros::init(argc, argv, "LidarCameraBoxMerge");

    ros::NodeHandle nh("~");

    // Do parameter stuff.
    {
        lidar2camera_R << 0.9994, -0.006152, 0.034226, -0.034151, 0.011869, 0.99935, -0.0065542, -0.99991, 0.01165;
        lidar2camera_T << -0.00305, 0.42377, -0.09468;
        vector<double> l2c_R, l2c_T;
        nh.param<vector<double>>("lidar2camera_T", l2c_T, l2c_T);
        nh.param<vector<double>>("lidar2camera_R", l2c_R, l2c_R);

        if (!l2c_R.empty()) {
            lidar2camera_R(0, 0) = l2c_R[0];
            lidar2camera_R(0, 1) = l2c_R[1];
            lidar2camera_R(0, 2) = l2c_R[2];
            lidar2camera_R(1, 0) = l2c_R[3];
            lidar2camera_R(1, 1) = l2c_R[4];
            lidar2camera_R(1, 2) = l2c_R[5];
            lidar2camera_R(2, 0) = l2c_R[6];
            lidar2camera_R(2, 1) = l2c_R[7];
            lidar2camera_R(2, 2) = l2c_R[8];
        }

        if (!l2c_T.empty()) {
            lidar2camera_T(0) = l2c_T[0];
            lidar2camera_T(1) = l2c_T[1];
            lidar2camera_T(2) = l2c_T[2];
        }

        dcamera2lidar_R << 1, 0, 0, 0, -1, 0, 0, 0, 1;
        dcamera2lidar_T << 0, 0, 0;
        vector<double> l2dc_R, l2dc_T;
        nh.param<vector<double>>("dcamera2lidar_T", l2dc_T, l2dc_T);
        nh.param<vector<double>>("dcamera2lidar_R", l2dc_R, l2dc_R);
        if (!l2dc_R.empty()) {
            dcamera2lidar_R(0, 0) = l2dc_R[0];
            dcamera2lidar_R(0, 1) = l2dc_R[1];
            dcamera2lidar_R(0, 2) = l2dc_R[2];
            dcamera2lidar_R(1, 0) = l2dc_R[3];
            dcamera2lidar_R(1, 1) = l2dc_R[4];
            dcamera2lidar_R(1, 2) = l2dc_R[5];
            dcamera2lidar_R(2, 0) = l2dc_R[6];
            dcamera2lidar_R(2, 1) = l2dc_R[7];
            dcamera2lidar_R(2, 2) = l2dc_R[8];
        }
        if (!l2dc_T.empty()) {
            dcamera2lidar_T(0) = l2dc_T[0];
            dcamera2lidar_T(1) = l2dc_T[1];
            dcamera2lidar_T(2) = l2dc_T[2];
        }


        nh.param<string>("WORK_SPACE_PATH", WORK_SPACE_PATH,
                         "/home/nuo/Projects/sjtu_obs-clu_ws/src/Obstacle-Cluster/Object_Cluster_voxelGrid");

        nh.param<double>("dcx", dcx);
        nh.param<double>("dcy", dcy);
        nh.param<double>("dfx", dfx);
        nh.param<double>("dfy", dfy);
        nh.param<double>("kScaleFactor", kScaleFactor);
        nh.param<float>("DeepCameraHigh", DeepCameraHigh);
        nh.param<float>("cx", cx);
        nh.param<float>("cy", cy);
        nh.param<float>("fx", fx);
        nh.param<float>("fy", fy);

        nh.param<string>("LidarBoxTopic", LidarBoxTopic);
        nh.param<string>("CameraBoxTopic", CameraBoxTopic);
        nh.param<int>("camera_cols", camera_cols);
        nh.param<int>("camera_rows", camera_rows);

        google::InitGoogleLogging(argv[0]);
        FLAGS_log_dir = WORK_SPACE_PATH + "/Log";
        FLAGS_alsologtostderr = true;
        FLAGS_colorlogtostderr = true;
        FLAGS_log_prefix = true;
        FLAGS_logbufsecs = 0;
        FileManager::CreateDirectory(FLAGS_log_dir);

    }


//    pub_UInt8MultiArray_ = nh.advertise<std_msgs::UInt8MultiArray>("/cloud_point_ai_UInt8MultiArray", 5);
//    pub_MergeLidarCameraBox_ = nh.advertise<msgs::BboxList>("/Merge_Lidar_Camera_Box_BboxList", 5);
    sub_LidarBox_ = nh.subscribe("/LidarObjBox", 1, LidarBoxHandler);  //LidarBoxTopic
    sub_Lidar_ = nh.subscribe<sensor_msgs::PointCloud2>("/TempPointCloud", 1, LidarHandler);  //LidarBoxTopic
    sub_CameraBox_ = nh.subscribe("/yolox_ros/bboxlist", 1, CameraBoxHandler);  //CameraBoxTopic
    sub_PCArray_obj_ = nh.subscribe("/PC_Array_obj", 1, PCArrayHandler);
    pub_vertical_view_ = nh.advertise<visualization_msgs::MarkerArray>("/vertical_view", 5);
    pub_vertical_view_text_ = nh.advertise<visualization_msgs::MarkerArray>("/vertical_view_text", 5);
    pub_vertical_view_bbox_ = nh.advertise<visualization_msgs::MarkerArray>("/vertical_view_bbox", 5);
    pub_vertical_view_coordinate_ = nh.advertise<msgs::BboxList>("/vertical_view_coordinate",5);

    ros::Rate rate(100);
    //bool status = ros::ok();
    while (ros::ok()) {
        ros::spinOnce();

//        if(newCameraBox = true){
//            newCameraBox = false;
//            if(LidarBoxArraySeq.empty())
//                continue;
//        }else{
//            continue;
//        }
        //cout << newLidarBox;
        if(newLidarBox){
            newLidarBox = false;
            if(CamBoxArraySeq.empty())
                continue;
        }else{
            continue;
        }

        //time matching
//        CamStamp.stamp.sec;
//        float minIntervalInd=0;
//        double minIntervalTime = abs(CamStamp.stamp.toSec() - LidarBoxStamp[0].stamp.toSec());
//        for(int i = 1;i<LidarBoxStamp.size();i++){
//            double IntervalTime = abs(CamStamp.stamp.toSec() - LidarBoxStamp[i].stamp.toSec());
//            if(IntervalTime < minIntervalTime){
//                minIntervalInd = i;
//                minIntervalTime = IntervalTime;
//            }
//        }
//        std::cout<<"Lidar Camera Box merge interval time:"<<minIntervalInd<<endl;
//        ROS_INFO("the size of LidarBoxArraySeq is : %zu",LidarBoxArraySeq.size());
//        vector<pair<pcl::PointXY,pcl::PointXY>> LidarBoxArray = LidarBoxArraySeq[minIntervalInd];

        int minIntervalInd=0;
        double minIntervalTime = abs(PC_Stamp.stamp.toSec() - CamBoxArraySeq[0].header.stamp.toSec());
        for(int i = 1;i<CamBoxArraySeq.size();i++){
            double IntervalTime = abs(PC_Stamp.stamp.toSec() - CamBoxArraySeq[i].header.stamp.toSec());
            if(IntervalTime < minIntervalTime){
                minIntervalInd = i;
                minIntervalTime = IntervalTime;
            }
        }
        std::cout<<"Lidar Camera Box merge index:"<<minIntervalInd<<endl;
        ROS_INFO("the size of CamBoxArraySeq is : %zu",CamBoxArraySeq.size());
        msgs::BboxList CamBoxList_ = CamBoxArraySeq[minIntervalInd];
        //ROS_INFO("the size of LidarBoxArray is : %zu",LidarBoxArray.size());
//        vector<pair<pcl::PointXY,pcl::PointXY>> LidarBoxArrayCopy = LidarBoxArraySeq[minIntervalInd];
        //ROS_INFO("the merge is beginning!");

        CamBoxArraySeq.erase(CamBoxArraySeq.begin(),CamBoxArraySeq.begin()+minIntervalInd);

        //  visualization_msgs::MarkerArray marker_array_text;
        visualization_msgs::MarkerArray marker_array_text_1;
        visualization_msgs::MarkerArray marker_array_bbox;
        int axisXmax, axisXmin, axisYmax, axisYmin;
        //遍历每个图像检测框，然后把点云向图像检测框中投影，寻找在图像检测框中的点云
        for(int i = 0;i<CamBoxList_.Bboxlist.size(); i++){
            int cminx = CamBoxList_.Bboxlist[i].bbox[0];
            int cminy = CamBoxList_.Bboxlist[i].bbox[1];
//            int cmaxx = CamBoxList.Bboxlist[i].bbox[1]+CamBoxList.Bboxlist[i].bbox[3];
//            int cmaxy = CamBoxList.Bboxlist[i].bbox[2]+CamBoxList.Bboxlist[i].bbox[4];
            int cmaxx = CamBoxList_.Bboxlist[i].bbox[2];
            int cmaxy = CamBoxList_.Bboxlist[i].bbox[3];

            pcl::PointCloud<pcl::PointXYZ>::Ptr laserCloud_middle (new pcl::PointCloud<pcl::PointXYZ>());
            int PointSize = laserCloud_temp->points.size();
            ROS_INFO("PointSize = %d",PointSize);
            for(int j =0;j<PointSize;j++){
                //thisPoint = thisCloud.points[i];
                int pixel_x = 0;
                int pixel_y = 0;

                float PX1 = laserCloud_temp->points[j].x;
                float PY1 = laserCloud_temp->points[j].y;
                float PZ1 = laserCloud_temp->points[j].z;

                //lidar->camera
                Eigen::Vector3d v3d;
                v3d<<PX1,PY1,PZ1;
                v3d = lidar2camera_R.transpose()*v3d + (-lidar2camera_R.transpose()*lidar2camera_T);

                //normalize
                v3d.x() = v3d.x()/v3d.z();
                v3d.y() = v3d.y()/v3d.z();
                v3d.z() = 1;

                //distort

                //camera->pixel
                pixel_x = fx*v3d.x()+cx;
                pixel_y = fy*v3d.y()+cy;
                if(cminx <= pixel_x &&  pixel_x <= cmaxx && cminy <= pixel_y &&  pixel_y <= cmaxy){
                    laserCloud_middle->push_back(laserCloud_temp->points[j]);
                }
            }

            if(laserCloud_middle->points.size() == 0){
                continue;}
            ROS_INFO("THE number of laserCloud_middle is : %zu",laserCloud_middle->points.size());
            cloudClusters = EuclideanClustering(laserCloud_middle);
            ROS_INFO("*****the number of cloudClusters is : %zu: ",cloudClusters.size());
            if (cloudClusters.size() == 0 ){continue;}
            int cluster_index = -1;
            int cluster_num = 0;
            //寻找点云数量最多的点云作为与图像检测框的匹配点云，此方式待商榷；
            for(int j = 0; j < cloudClusters.size();j++ ){

                if(cluster_num < cloudClusters[j]->points.size()){
                    cluster_index = j;
                    cluster_num = cloudClusters[j]->points.size();
                }
            }
            if (cluster_index == -1 ){continue;}
            pcl::PointXYZ minPoint_box_, maxPoint_box_;
            pcl::getMinMax3D(*cloudClusters[cluster_index], minPoint_box_, maxPoint_box_);  //得到聚类物体的正方体的最小值点和最大值点
            Eigen::Vector4d centroid_obj;
            pcl::compute3DCentroid (*cloudClusters[cluster_index], centroid_obj);  //得到点云的质心

            float PC_distance = sqrt(centroid_obj[0]*centroid_obj[0] + centroid_obj[1]*centroid_obj[1] + centroid_obj[2]*centroid_obj[2]);
            ROS_INFO("...........THE points_size of cloudCluster is : %zu",cloudClusters[cluster_index]->points.size());
            ROS_INFO("...........THE distance of cloudCluster is : %f",PC_distance);

            //发布文本话题
            visualization_msgs::Marker marker_text_1;
            marker_text_1.header.frame_id="/map";
            marker_text_1.header.stamp = ros::Time::now();
            marker_text_1.ns = "basic_shapes";
            marker_text_1.action = visualization_msgs::Marker::ADD;
            marker_text_1.pose.orientation.w = 1.0;
            marker_text_1.id = num_text++;
            marker_text_1.type = visualization_msgs::Marker::TEXT_VIEW_FACING;

            marker_text_1.scale.z = 0.5;
            marker_text_1.color.b = 255;
            marker_text_1.color.g = 255;
            marker_text_1.color.r = 255;
            marker_text_1.color.a = 1;

            marker_text_1.lifetime = ros::Duration(0.35);
            geometry_msgs::Pose pose_text_1;
            pose_text_1.position.x =  centroid_obj[0];
            pose_text_1.position.y =  centroid_obj[1];
            pose_text_1.position.z =0.3;
            marker_text_1.pose = pose_text_1;
            marker_text_1.text +=  "class:" + CamBoxList_.Bboxlist[i].label +
                                 ",\n score:" + to_string(CamBoxList_.Bboxlist[i].conf);
            marker_array_text_1.markers.push_back(marker_text_1);

            //发布bbox话题
            visualization_msgs::Marker bbox_marker;
            bbox_marker.header.frame_id = "map";  //camera_init
            bbox_marker.ns = "";
            if(CamBoxList_.Bboxlist[i].label == "person"){
                bbox_marker.color.r = 1.0f;
                bbox_marker.color.g = 0.0f;
                bbox_marker.color.b = 0.0f;
            } else if(CamBoxList_.Bboxlist[i].label == "chair"){
                bbox_marker.color.r = 0.0f;
                bbox_marker.color.g = 0.0f;
                bbox_marker.color.b = 1.0f;
            }else{
                bbox_marker.color.r = 0.0f;
                bbox_marker.color.g = 1.0f;
                bbox_marker.color.b = 0.0f;
            }
            bbox_marker.color.a = 0.5;
            bbox_marker.lifetime = ros::Duration();
            bbox_marker.frame_locked = true;
            bbox_marker.type = visualization_msgs::Marker::CUBE;
            bbox_marker.action = visualization_msgs::Marker::ADD;

            bbox_marker.id = i;
            bbox_marker.color.a = 0.5;
            bbox_marker.pose.position.x = centroid_obj[0];
            bbox_marker.pose.position.y = centroid_obj[1];
            bbox_marker.pose.position.z = centroid_obj[2];
            bbox_marker.scale.x = maxPoint_box_.x - minPoint_box_.x;
            bbox_marker.scale.y = maxPoint_box_.y - minPoint_box_.y;
            bbox_marker.scale.z = maxPoint_box_.z - minPoint_box_.z;
            bbox_marker.header.stamp = ros::Time::now();
            marker_array_bbox.markers.push_back(bbox_marker);

//            for (int j = 0; j < marker_array_merge.markers.size(); j++) {
//                marker_array_merge.markers[j].scale.z = 0.2;
//            }
            //pub_vertical_view_.publish(marker_array_merge);
            cloudClusters.clear();
            laserCloud_middle->clear();

        }
        pub_vertical_view_text_.publish(marker_array_text_1);
        pub_vertical_view_bbox_.publish(marker_array_bbox);
        LidarBoxArray.clear();

        laserCloud_temp->clear();


//            float iou_max = 0.0;
//            int iou_index = -1;
//            //如果 iou 本身占比较大，可认为是同一物体，给点云聚类物体添加标签
//            visualization_msgs::Marker marker_text;
//            marker_text.header.frame_id="/map";
//            marker_text.header.stamp = ros::Time::now();
//            marker_text.ns = "basic_shapes";
//            marker_text.action = visualization_msgs::Marker::ADD;
//            marker_text.pose.orientation.w = 1.0;
//            marker_text.id = num_text++;
//            marker_text.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
//
//            marker_text.scale.z = 0.5;
//            marker_text.color.b = 255;
//            marker_text.color.g = 255;
//            marker_text.color.r = 255;
//            marker_text.color.a = 1;
//
//            marker_text.lifetime = ros::Duration(0.35);
//
//
//            //ROS_INFO("cminx:%d ,cminy:%d , cmaxx:%d, cmaxy:%d", cminx,cminy,cmaxx,cmaxy);
//            //ROS_INFO("the index of CamBoxList is : %d", i);
//            for(int j = 0;j<LidarBoxArray.size();j++){
//                int lminx = LidarBoxArray[j].first.x;
//                int lminy = LidarBoxArray[j].first.y;
//                int lmaxx = LidarBoxArray[j].second.x;
//                int lmaxy = LidarBoxArray[j].second.y;
//                //ROS_INFO("lminx:%d ,lminy:%d , lmaxx:%d, lmaxy:%d", lminx,lminy,lmaxx,lmaxy);
//                //寻找相机和激光的交集
//                lminx < cminx ? axisXmin = cminx : axisXmin = lminx;
//                lmaxx < cmaxx ? axisXmax = lmaxx : axisXmax = cmaxx;
//                if(axisXmax - axisXmin < 0)continue;
//                lminy < cminy ? axisYmin = cminy : axisYmin = lminy;
//                lmaxy < cmaxy ? axisYmax = lmaxy : axisYmax = cmaxy;
//                if(axisYmax - axisYmin < 0)continue;
//                //ROS_INFO("axisXmin:%d ,axisYmin:%d , axisXmax:%d, axisYmax:%d", axisXmin,axisYmin,axisXmax,axisYmax);
//                //计算IOU
//                int camera_square = (cmaxx - cminx)*(cmaxy - cminy);
//                int lidar_square = (lmaxx - lminx)*(lmaxy - lminy);
//                int iou_square = (axisXmax - axisXmin)*(axisYmax - axisYmin);
//                // float iou = (axisXmax - axisXmin)*(axisYmax - axisYmin)/((cmaxx-cminx)*(cmaxy-cminy)+(lmaxx-lminy)*(lmaxy-lminy)-(axisXmax - axisXmin)*(axisYmax - axisYmin));
//                float iou = (float)iou_square / float(camera_square + lidar_square - iou_square);
//                float iou_in_image = (float)iou_square / float(camera_square);
//                ROS_INFO("-------the iou is : %f", iou);
//                ROS_INFO("-------the iou_in_image is : %f", iou_in_image);
//
//
//                if(iou_in_image > 0){
//                    //lidar overlap with camera
////                    LidarBoxArray.erase(LidarBoxArray.begin()+j);
////                    j--;
////                    marker_text.text +=  "class:" + CamBoxList_.Bboxlist[i].label +
////                                                           "score:" + to_string(CamBoxList_.Bboxlist[i].conf);
//                    //continue;
//                    if(iou_max < iou_in_image){
//                        iou_index = j;
//                        iou_max = iou_in_image;
//                    }
//                }
////                // 如果 iou 公共占比较小，但在图片上的占比较大，可认为 点云聚类 的物体距离比较近，多个物体聚类成一个物体,添加标签
////                else if(iou/camera_square > 0.2 )  {
////                    marker_text.text +=  "class:" + CamBoxList_.Bboxlist[i].label +
////                                                           "score:" + to_string(CamBoxList_.Bboxlist[i].conf);
////                    //continue;
////                }
//                else{
//                    //marker_text.text = "0";
//                    continue;
//                }
//            }
//            if(iou_index == -1)continue; //如果iou都太小的话，则iou_index不变，直接跳过
//            ROS_INFO("--------the value of iou_max is : %f",iou_max);
//            geometry_msgs::Pose pose_text;
//            pose_text.position.x =  marker_array_merge.markers[iou_index].pose.position.x;
//            pose_text.position.y =  marker_array_merge.markers[iou_index].pose.position.y;
//            pose_text.position.z =0.3;
//            marker_text.pose = pose_text;
//            marker_text.text +=  "class:" + CamBoxList_.Bboxlist[i].label +
//                                 ",\n score:" + to_string(CamBoxList_.Bboxlist[i].conf);
//            marker_array_text.markers.push_back(marker_text);
//        }
//        //ROS_INFO("Merge is ending!");
////        msgs::Bbox cambox;
////        for(int i = 0;i<LidarBoxArray.size();i++){
////            cambox.conf = 1;
////            cambox.label = "lidar object";
////            cambox.bbox[0] = LidarBoxArray[i].first.x;
////            cambox.bbox[1] = LidarBoxArray[i].first.y;
//////            cambox.bbox[2] = LidarBoxArray[i].second.x - LidarBoxArray[i].second.x;
//////            cambox.bbox[3] = LidarBoxArray[i].second.y - LidarBoxArray[i].second.y;
////            cambox.bbox[2] = LidarBoxArray[i].second.x ;
////            cambox.bbox[3] = LidarBoxArray[i].second.y ;
////            CamBoxList_.Bboxlist.push_back(cambox);
////        }
////        pub_MergeLidarCameraBox_.publish(CamBoxList_);
//        for (int i = 0; i < marker_array_merge.markers.size(); i++) {
//            marker_array_merge.markers[i].scale.z = 0.2;
//        }
//        pub_vertical_view_.publish(marker_array_merge);
//        pub_vertical_view_text_.publish(marker_array_text);
//
//        LidarBoxArray.clear();
    }
}